#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <serial/serial.h>
#include <customized_msgs/msg/feed_back.hpp>
#include <queue>
#include <geometry_msgs/msg/point.hpp>
#include <std_msgs/msg/empty.hpp>
#include <chrono>
#include <customized_msgs/msg/gcs_cmd.hpp>
// #include <customized_msgs/msg/drone_state_gcs.hpp>
#include <customized_msgs/msg/topic_frequencies.hpp>
#include <customized_msgs/msg/winch_state.hpp>
#include "custom_filters/MeanFilter.hpp"
#include <customized_msgs/msg/ground_detection.hpp>
#include <sensor_msgs/msg/range.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <customized_msgs/msg/arg_set_length.hpp>
#include "can_motor_driver/crc_checker.hpp"
#include <customized_msgs/msg/drone_whole_state.hpp>
#include <iomanip>  // for std::setw and std::setfill
#include <vector>
// #define SERIAL_DEBUG true
#define SERIAL_DEBUG false

#define GCS_ENABLE true

#define ANSI_COLOR_RED "\x1b[31m"
#define ANSI_COLOR_GREEN "\x1b[32m"
#define ANSI_COLOR_YELLOW "\x1b[33m"
#define ANSI_COLOR_RESET "\x1b[0m"





class WinchWirelessServer : public rclcpp::Node
{
public:
    WinchWirelessServer() : Node("winch_wireless_server"), ser(), crc_checker()
    {
        this->declare_parameter<std::string>("serial_port_name", "/dev/ttyUSB0");
        this->declare_parameter<double>("control_frequency", 5.0);

        this->get_parameter("serial_port_name", serial_port_name);
        this->get_parameter("control_frequency", control_frequency);
        
        RCLCPP_WARN(this->get_logger(), "starting winch_wireless_server at %f hz @%s port",control_frequency, serial_port_name.c_str());

        // timer = this->create_wall_timer(
        //     std::chrono::duration<double>(1.0 / control_frequency),
        //     std::bind(&WinchWirelessServer::timerCallback, this));

        motor_fb_sub = this->create_subscription<customized_msgs::msg::FeedBack>(
            "/motor_feedback/motor1_winch", 10, std::bind(&WinchWirelessServer::motor_fb_callback, this, std::placeholders::_1));
        gcs_cmd_sub = this->create_subscription<customized_msgs::msg::GcsCmd>(
            "/winch_wireless_server/gcs_cmd", 10, std::bind(&WinchWirelessServer::gcs_cmd_callback, this, std::placeholders::_1));

        drone_whole_state_sub = this->create_subscription<customized_msgs::msg::DroneWholeState>(
             "/winch_wireless_server/drone_whole_state", 10, std::bind(&WinchWirelessServer::drone_whole_state_callback, this, std::placeholders::_1));

       // gcs_cmd_pub = this->create_publisher<customized_msgs::msg::GcsCmd>("/winch_wireless_server/gcs_cmd", 1000);
        odom_pub = this->create_publisher<nav_msgs::msg::Odometry>("/winch_wireless_server/grabber_odom", 1000);
        winch_length_pub = this->create_publisher<customized_msgs::msg::ArgSetLength>("/winch_wireless_server/set_length", 1000);
        winch_state_sub = this->create_subscription<customized_msgs::msg::WinchState>(
            "/winch_ctrl_winch/winch_state", 10, std::bind(&WinchWirelessServer::winch_state_callback, this, std::placeholders::_1));
        ground_height_pub = this->create_publisher<customized_msgs::msg::GroundDetection>("/ground_detector/ground_height_winch", 1000);
        // drone_state_pub = this->create_publisher<customized_msgs::msg::DroneStateGcs>("/winch_wireless_server/drone_state", 1000);
        topic_freq_pub = this->create_publisher<customized_msgs::msg::TopicFrequencies>("/winch_wireless_server/topic_frequencies", 1000);

        timer = this->create_wall_timer(
        std::chrono::duration<double>(1.0 / control_frequency),
        std::bind(&WinchWirelessServer::timerCallback, this));

        motor_fb_msg.feedback_angle = uninitialized_double;
        motor_fb_msg.feedback_velocity = uninitialized_double;
        motor_fb_msg.feedback_torque = uninitialized_double;
        motor_fb_msg.mos_temperature = uninitialized_int;
        motor_fb_msg.rotor_temperature = uninitialized_int;
        winch_state_msg.target_length = uninitialized_double;

        // last_recv_grabber_cmd_time = this->now();
        // if (!ser.isOpen())
        // {
        //     try
        //     {
        //         OpenSerial();
        //     }
        //     catch (const serial::IOException &e)
        //     {
        //         RCLCPP_ERROR(this->get_logger(), "Unable to open port ");
        //         //  RCLCPP_INFO(this->get_logger(), "no wireless message recved!!! ");
        //         customized_msgs::msg::TopicFrequencies topic_freq_msg;
        //         topic_freq_msg.gcs_freq = 0.0000;
        //         topic_freq_pub->publish(topic_freq_msg);
        //     }
        // }

        // if (ser.isOpen())
        // {
        //     try
        //     {
        //         update_send_data();
        //     }
        //     catch (const serial::SerialException &e)
        //     {
        //         RCLCPP_ERROR(this->get_logger(), "Serial error");
        //     }
        // }
    }
    void timerCallback()
    {
        rclcpp::Time now = this->now();
        // RCLCPP_INFO(this->get_logger(), "call back!!!");
        // RCLCPP_INFO(this->get_logger(),"stick is %f ",(last_recv_drone_state_time-now).seconds());
        if ((now-last_recv_drone_state_time).seconds()>1)
        {
            customized_msgs::msg::TopicFrequencies topic_freq_msg;
            topic_freq_msg.gcs_freq = 0.0;
            topic_freq_pub->publish(topic_freq_msg);
            // RCLCPP_INFO(this->get_logger(), "pub 0!");
        }
        return;
    }
    void winch_state_callback(const customized_msgs::msg::WinchState::SharedPtr msg)
    {
        winch_state_msg = *msg;
    }

    void motor_fb_callback(const customized_msgs::msg::FeedBack::SharedPtr msg)
    {
        motor_fb_msg = *msg;
    }
    void gcs_cmd_callback(const customized_msgs::msg::GcsCmd::SharedPtr msg)
    {
        gcs_cmd_msg = *msg;
        gcs_cmd_msg_init_flag = true;
    }

    bool check_double_equal(double a,double b)
    {
        const double precision = 0.00001;
        if(fabs(a-b) < precision)
        {
            return true;
        }
        else
        {
            return false;
        }
    }


    void drone_whole_state_callback(const customized_msgs::msg::DroneWholeState::SharedPtr msg)
    {
        
        double odom_x = msg->odom_x;
        double odom_y = msg->odom_y;
        double odom_z = msg->odom_z;

        if(!check_double_equal(odom_x,uninitialized_double) || !check_double_equal(odom_y,uninitialized_double) || !check_double_equal(odom_z,uninitialized_double))
        // if (odom_x != uninitialized_double || odom_y != uninitialized_double || odom_z != uninitialized_double)
        {
            nav_msgs::msg::Odometry odom_msg;
            odom_msg.header.stamp = this->now();
            odom_msg.header.frame_id = "odom";
            odom_msg.child_frame_id = "base_link";
            odom_msg.pose.pose.position.x = odom_x;
            odom_msg.pose.pose.position.y = odom_y;
            odom_msg.pose.pose.position.z = odom_z;
            odom_pub->publish(odom_msg);
        }
        else
        {
            // RCLCPP_INFO(this->get_logger(), "uninitialized odom data");
        }

        double winch_length_control = msg->winch_length_control;
        int winch_length_control_flag = msg->winch_length_control_flag;

        if (fabs(winch_length_control) > 0.0000001 || winch_length_control_flag)
        {
            customized_msgs::msg::ArgSetLength set_length_msg;
            set_length_msg.length = winch_length_control;
            set_length_msg.abs_ctrl = winch_length_control_flag;
            set_length_msg.delay_time = 0;
            winch_length_pub->publish(set_length_msg);
        }
        // customized_msgs::msg::DroneWholeState drone_state_msg;


        double ground_height = msg->ground_height;

        // if (ground_height != uninitialized_double)
        // {
        //     customized_msgs::msg::GroundDetection ground_height_msg;
        //     ground_height_msg.filtered_height = ground_height;
        //     ground_height_msg.stamp = this->now();
        //     ground_height_pub->publish(ground_height_msg);
        // }
        // else
        // {
        //     // ROS_INFO("uninitialized ground height data");
        // }
        // if(!check_double_equal(odom_x,uninitialized_double) || !check_double_equal(odom_y,uninitialized_double) || !check_double_equal(odom_z,uninitialized_double))
        // // if (odom_x != uninitialized_double || odom_y != uninitialized_double || odom_z != uninitialized_double)
        // {
        //     drone_state_msg.tracking_error = msg->tracking_error;
        //     drone_state_msg.grab_process = msg->grab_process;
        //     drone_state_msg.tag_seen_flag = msg->tag_seen_flag;
        //     drone_state_msg.arm_state = msg->arm_state;
        //     drone_state_msg.battery_voltage = msg->battery_voltage;
        //     drone_state_msg.stamp = this->now();
        //     drone_state_msg.odom_x = odom_x;
        //     drone_state_msg.odom_y = odom_y;
        //     drone_state_msg.odom_z = odom_z;
        //     drone_state_pub->publish(drone_state_msg);
        // }
            

        customized_msgs::msg::TopicFrequencies topic_freq_msg;
        topic_freq_msg.gcs_freq = msg->gcs_freq;
        topic_freq_pub->publish(topic_freq_msg);

        // customized_msgs::msg::GcsCmd gcs_cmd_msg;
        // gcs_cmd_msg.cmd = override_cmd;
        // gcs_cmd_msg.winch_speed = manual_winch_speed;
        // gcs_cmd_msg.stamp = this->now();
        // gcs_cmd_pub->publish(gcs_cmd_msg);

        last_recv_drone_state_time = this->now();
        // ROS_INFO("target_angle = %f, target_velocity = %f, cmd = %d",target_angle,target_velocity,cmd);
    }


private:
    rclcpp::TimerBase::SharedPtr timer;

    std::string serial_port_name;
    double control_frequency;

    serial::Serial ser;

    rclcpp::Publisher<customized_msgs::msg::GcsCmd>::SharedPtr gcs_cmd_pub;

    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
    rclcpp::Publisher<customized_msgs::msg::ArgSetLength>::SharedPtr winch_length_pub;
    rclcpp::Publisher<customized_msgs::msg::GroundDetection>::SharedPtr ground_height_pub;
    // rclcpp::Publisher<customized_msgs::msg::DroneStateGcs>::SharedPtr drone_state_pub;
    rclcpp::Publisher<customized_msgs::msg::TopicFrequencies>::SharedPtr topic_freq_pub;

    rclcpp::Subscription<customized_msgs::msg::WinchState>::SharedPtr winch_state_sub;
    rclcpp::Subscription<customized_msgs::msg::FeedBack>::SharedPtr motor_fb_sub;
    rclcpp::Subscription<customized_msgs::msg::GcsCmd>::SharedPtr gcs_cmd_sub;
    rclcpp::Subscription<customized_msgs::msg::DroneWholeState>::SharedPtr drone_whole_state_sub;

    customized_msgs::msg::FeedBack motor_fb_msg;
    customized_msgs::msg::WinchState winch_state_msg;
    customized_msgs::msg::GcsCmd gcs_cmd_msg;
    bool gcs_cmd_msg_init_flag = false;
    double grabber_frequency = 0.0;

    int grabber_pack_length = 22 + 9;

    const double uninitialized_double = 0.12;
    const int uninitialized_int = 123;
    
    uint8_t winch_send_data[17] = {0xDD, 0x00, 0x01, 0x02, 0x01, 0x02, 0x01, 0x02, 0x01, 0x02, 0x03, 0x03, 0x01, 0x02, 0x03, 0x04, 0xEE};
    int winch_send_data_length = 17;

    std::queue<uint8_t> recv_can_data_queue;

    CrcChecker crc_checker;

    rclcpp::Time last_recv_drone_state_time = this->now();
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<WinchWirelessServer>());
    rclcpp::shutdown();
    return 0;
}
